using System;
using System.Threading;
using LegoBotPrimaAbstractions;
using Microsoft.Robotics.Services.Drive.Proxy;

namespace LegoBotPrima.Actions
{
    public class ForwardBotAction : BotAction
    {

        private static readonly object _synch_OnDistanceChanged = new object();
        private static readonly object _synch_OnTimerTick = new object();
        private static readonly object _synch_OnAccelerometerChanged = new object();

        private double _previousAccelerationX;
        private double _previousAccelerationY;
        private double _previousAccelerationZ;

        private DateTime _previousTimerTimeStamp = DateTime.MinValue;
        private int _tickNumber;

        
        public override void Initialize()
        {
            Workflow.GoForwardFast();
        }

        
        public override void OnTimerTick(int tickNimber)
        {
            if(!IsRunning)
            {
                return;
            }
            lock (_synch_OnTimerTick)
            {
                if (this._tickNumber == 0)
                {
                    this._previousTimerTimeStamp = DateTime.Now;
                }

                // each 10 seconds check if the drive does not execute and there is no fault response from the driver either
                if (this._tickNumber % SecondsAsTicks(10) == 0)
                {
                    Workflow.GetDrivePortState(ReceivedDriveState, f => Workflow.Error("Error receiving drive state", f));
                }

                this._tickNumber++;
                var timeStamp = this._previousTimerTimeStamp.AddMilliseconds(100);
                var timeDiffSinceLast = timeStamp - this._previousTimerTimeStamp;
                var distanceDriven = (LegoBotPrimaWorkflow.Forward_Avg * timeDiffSinceLast.TotalMilliseconds) / 1000.0;
                var angle = (Workflow.RotationAngle + Config.Instance.InitialAngle) % 360;
                var x = distanceDriven * Math.Cos(MathUtil.DegreesToRadians(angle));
                var y = distanceDriven * Math.Sin(MathUtil.DegreesToRadians(angle));
                Workflow.DrawBotPositionChangeOnMap(x, y);

                this._previousTimerTimeStamp = timeStamp;

                // forward action has a minute limit
                if (_tickNumber % SecondsAsTicks(Config.Instance.MaxSecondsGoingForward) == 0)
                {
                    Console.WriteLine("Max going forward reached");
                    BackUp();
                }
            }
        }

        public override void OnAccelerometerChanged(double x, double y, double z, DateTime timeStamp)
        {
            if (!IsRunning)
            {
                return;
            }
            lock (_synch_OnAccelerometerChanged)
            {
                if (IsChanged(this._previousAccelerationX, x) || IsChanged(this._previousAccelerationY, y) || IsChanged(this._previousAccelerationZ, z))
                {
                    Console.WriteLine("Acceleration change detected - Prev X:" + this._previousAccelerationX + " X:" + x +
                                        " Prev Y:" + this._previousAccelerationX + " Y:" + y +
                                        " Prev Z:" + this._previousAccelerationZ + " Z:" + z 
                                        );

                    BackUp();
                    return;
                }
                this._previousAccelerationX = x;
                this._previousAccelerationY = y;
                this._previousAccelerationZ = z;
            }
        }

        private bool IsChanged(double  prev, double  curr)
        {
            return prev != 0 && Math.Abs(prev - curr) > Config.Instance.AccelerometerThreashold;
        }

        private void ReceivedDriveState(DriveDifferentialTwoWheelState state)
        {
            if (state.LeftWheel.MotorState.CurrentPower == 0 || state.RightWheel.MotorState.CurrentPower == 0)
            {
                RequestedNextAction = new Rotate360BotAction();
                Workflow.Stop();
                Workflow.EndAction(this);
            }
        }

        private void BackUp()
        {
            RequestedNextAction = new StepBackBotAction();
            Workflow.Stop();
            Workflow.EndAction(this);
        }

        public override void OnDistanceChanged(int distance, DateTime timeStamp)
        {
            if (!IsRunning)
            {
                return;
            }
            lock (_synch_OnDistanceChanged)
            {

                if (distance < (Config.Instance.MinDistance * Config.Instance.MinDistanceBackUpCoeff))
                {
                    this.RequestedNextAction = new StepBackBotAction();

                    Workflow.Stop();
                    Workflow.EndAction(this);
                }
                else if (distance < Config.Instance.MinDistance)
                {
                    this.RequestedNextAction = new WaitBotAction { Seconds = 1, AfterWaitAction = new Rotate360BotAction() };

                    Workflow.Stop();
                    Workflow.EndAction(this);
                }
            }
        }
    }
}